/**
 *
 */
package jp.seraph.jspf.pose;

import java.util.Arrays;
import java.util.Collection;
import java.util.HashMap;
import java.util.Iterator;
import java.util.List;
import java.util.Map;
import java.util.Map.Entry;

import jp.seraph.jsade.math.Angle;
import jp.seraph.jsade.model.Joint;
import jp.seraph.jsade.model.JointIdentifier;
import jp.seraph.jsade.model.JointIdentifierContainer;
import jp.seraph.jsade.model.ModelManager;

/**
 *
 *
 */
public class DefaultPose implements Pose {
    public DefaultPose() {
        mAngleMap = new HashMap<JointIdentifier, Angle>();
    }

    /**
     * 指定されたコンテナに存在するすべてのジョイントに対して角度0を設定したポーズを作成する
     *
     * @param aIdentifierContainer
     */
    public DefaultPose(JointIdentifierContainer aIdentifierContainer) {
        this();
        for (JointIdentifier tJointIdentifier : aIdentifierContainer) {
            this.setJointAngle(tJointIdentifier, Angle.ZERO);
        }
    }

    public DefaultPose(ModelManager aManager) {
        this();
        List<Joint> tJoints = aManager.getAllJoints();
        for (Joint tJoint : tJoints) {
            this.setJointAngle(tJoint.getIdentifier(), tJoint.getAngle());
        }
    }

    /**
     * 渡されたポーズのコピーを作成する
     *
     * @param aBasePose
     */
    public DefaultPose(Pose aBasePose) {
        this();

        for (Entry<JointIdentifier, Angle> tEntry : aBasePose) {
            mAngleMap.put(tEntry.getKey(), tEntry.getValue());
        }
    }

    private Map<JointIdentifier, Angle> mAngleMap;

    /**
     *
     * @see jp.seraph.jspf.pose.Pose#getJointAngle(jp.seraph.jsade.model.JointIdentifier)
     */

    public Angle getJointAngle(JointIdentifier aIdentifier) {
        Angle tResult = mAngleMap.get(aIdentifier);
        if (tResult == null)
            return Angle.ZERO;
        else
            return tResult;
    }

    /**
     *
     * @see jp.seraph.jspf.pose.Pose#filter(java.util.Collection)
     */
    public Pose filter(Collection<JointIdentifier> aIdentifiers) {
        DefaultPose tResult = new DefaultPose();
        for (JointIdentifier tId : aIdentifiers) {
            Angle tAngle = this.getJointAngle(tId);
            if(tAngle != null)
                tResult.setJointAngle(tId, tAngle);
        }

        return tResult;
    }

    /**
     *
     * @see jp.seraph.jspf.pose.Pose#filter(jp.seraph.jsade.model.JointIdentifier[])
     */
    public Pose filter(JointIdentifier... aIdentifiers) {
        return this.filter(Arrays.asList(aIdentifiers));
    }

    public void setJointAngle(JointIdentifier aIdentifier, Angle aAngle) {
        mAngleMap.put(aIdentifier, aAngle);
    }

    public void setJointAngle(JointIdentifier aIdentifier, double aDgreeAngle) {
        this.setJointAngle(aIdentifier, Angle.createAngleAsDegree(aDgreeAngle));
    }

    public void modifyPose(Pose aPose) {
        for (Entry<JointIdentifier, Angle> tEntry : aPose) {
            this.setJointAngle(tEntry.getKey(), tEntry.getValue());
        }
    }

    /**
     *
     * @see java.lang.Iterable#iterator()
     */

    public Iterator<Map.Entry<JointIdentifier, Angle>> iterator() {
        return mAngleMap.entrySet().iterator();
    }

    public Collection<JointIdentifier> jointCollection() {
        return mAngleMap.keySet();
    }


}
